#include "stackplate.h"

#include "systemconfiguration.h"
#include "component_log.h"
#include "componentmanager.h"
#include "component_log.h"
#include "../algorithm/AlgorithmManager.h"

#include "stackplatesql.h"

using namespace plate;
StackPlate::StackPlate()
{

}

StackPlate::~StackPlate()
{}

/*码垛算法参数*/
json_struct(StackCfgInput,
	mapping(int, dirOdd);
mapping(int, dirEven);
prop(&dirOdd, &dirEven);
);

bool StackPlate::Init(const plate::PlateInfo& param)
{
	m_id = param.id;
	if (m_id.isEmpty()) {
		log_error("stack plate id is null, init failed");
		return false;
	}


	m_di = param.di;
	m_rob = component->GetComponent(Component::component_robot);
	if (m_rob == nullptr) {
		log_error("{} init failed, get robot componet failed", m_id);
		return false;
	}

	auto algManger = dynamic_cast<AlgorithmManager*>(component->GetComponent(Component::component_algorithm));
	if (algManger == nullptr) {
		log_error("{} init failed, get alg manager componet failed", m_id);
		return false;
	}

	m_alg = algManger->GetAlgObjectByName(param.alg);
	if (m_alg == nullptr) {
		log_error("{} init failed, get alg [{}]failed", m_id, param.alg);
		return false;
	}
	AlgInit algparam = {};
	algparam.base = param.base;
	algparam.rang = param.rang;
	StackCfgInput cfg = {};
	cfg.dirEven = param.dirEven;
	cfg.dirOdd = param.dirOdd;
	QString other = cfg.toByteArray();
	if (!m_alg->Init(algparam, other)) {
		log_error("{} init failed, alg [{}] init failed", m_id, param.alg);
		return false;
	}
	InitPlateState();
	log_trace("{} init successed", m_id);
	return true;
}

bool StackPlate::ManualChangePlateState(plate::PlateState& info)
{
	log_trace("info plate from {} to {}, num form {} to {}", m_state.plate ? "true" : "false",
		info.plate ? "true" : "false", m_state.num, info.num);

	if (!CheckDI(info.plate)) {
		log_error("DI check ERROR");
		return false;
	}

	if (!info.plate) {
		m_state.clear();
	} else {
		m_state.plate = true;
		m_state.num = info.num;
		if (m_state.num == 0) {
			m_state.sku.clear();
		}
	}
	SavePlateInfoToSql();
	return true;
}

/*获取托盘物料信息*/
plate::PlateState StackPlate::GetPlateStateInfo()
{
	return m_state;
}

// 设置托盘状态为到位状态
bool StackPlate::SetPlateArrived()
{
	if (!CheckDI(true)) {
		log_error("DI check failed");
		return false;
	}

	if (m_state.plate) {
		log_error("{} plate state is true before", m_id);
		return true;
	}
	m_state.num = 0;
	m_state.plate = true;
	SavePlateInfoToSql();
	return true;
}

// 设置托盘占用
bool StackPlate::GetPlateBusyState()
{
	return m_busy;
}

bool StackPlate::SetPlateBusy()
{
	if (m_busy) {
		log_error("plate is busy, can not set busy too");
		return false;
	}
	m_busy = true;
	log_trace("plate {} set to busy", m_id);
	return true;
}
bool StackPlate::SetPlateRelease()
{
	if (!m_busy) {
		log_error("plate is not busy, can not release agin");
		return false;
	}
	m_busy = false;
	log_trace("plate {} set to busy release", m_id);
	return true;
}

bool StackPlate::SetPlateMoved()
{
	if (!CheckDI(false)) {
		log_error("DI check failed");
		return false;
	}
	if (m_state.plate) {
		log_trace("{} plate state change to false", m_id);
	} else {
		log_error("{} plate state is false before", m_id);
	}
	m_state.clear();
	SavePlateInfoToSql();
	return true;
}

bool StackPlate::InitAlgSkuInfo()
{
	AlgSkuInfo algparam = {};
	algparam.size = m_state.sku.size;
	algparam.stackType = m_state.sku.pos;
	QString result;
	bool ret = m_alg->InitSkuInfo(algparam, result);
	if (ret) {	// 算法初始化物料信息成功, 解析垛形信息
		AlgStackInfo stack = {};
		stack.fromJson(result.toUtf8());
		m_state.stack.type = stack.id();
		m_state.stack.name = stack.name();
		m_state.stack.ceng = stack.ceng();
		m_state.stack.max = stack.max();
		return true;
	} else {
		log_error("{} alg init skuinfo failed, err = {}", result);
		return false;
	}
}

// 托盘货物信息处理，更新托盘上的货物信息
bool StackPlate::UpdataPlateSkuInfo(const SkuInfo& sku, int stackType)
{
	if (!sku.valied) {
		log_error("Sku info is not valied, can not update to plate {}", m_id);
		return false;
	}

	if (m_state.sku.valied) {
		log_error("{} sku info is not null before, now update agin, before info({},{},{}) ",
			m_id, sku.size.length, sku.size.width, sku.size.height);
	}
	m_state.sku.clear();
	m_state.stack.clear();
	m_state.sku = sku;
	m_state.sku.pos = stackType;
	log_trace("{} updata sku to ({},{},{})", m_id, sku.size.length, sku.size.width, sku.size.height);
	// SavePlateInfoToSql();
	return InitAlgSkuInfo();
}
// 放货通知
bool StackPlate::AddSkuToPlate(int num)
{
	if (!m_state.plate || !m_state.sku.valied) {
		log_error("{} plate isn't arrive or sku info is null, unable to add sku to plate", m_id);
		return false;
	}
	int add = m_state.num + num;
	log_trace("{} sku num form {} to {}", m_id, m_state.num, num);
	m_state.num = add;
	SavePlateInfoToSql();
	return true;
}

/*获取托盘到位状态， true:托盘到位*/
bool StackPlate::GetPlateState()
{
	return m_state.plate;
}
/*获取托盘上物料数量， -1 表示托盘未到位，>=0表示托盘上物料数量*/
int StackPlate::GetPlateSkuNum()
{
	return m_state.num;
}

/*算法计算下一次抓取数据，index:抓取次序 plan：计划放货数量*/
bool StackPlate::CalculateNextGrab(int index, int plan, QString& info)
{
	if (!m_state.sku.valied) {
		info = "码垛算法未初始化物料信息，无法计算";
		return false;
	}
	if (m_state.stack.type > 0 && m_state.stack.max > 0) {	// 垛形信息有效
		if (m_state.num >= m_state.stack.max) {
			info = "托盘已满，无法生成计划抓取数据";
			return false;
		}
	}

	AlgCalParam param = {};
	param.num = 0;
	param.grabed = m_state.num;
	QString result;
	bool ret = m_alg->ExcuteCmd(1, param.toByteArray(), result);
	if (ret) {
		StackAlgResult ret = {};
		ret.fromJson(result.toUtf8());
		log_trace("{} Get next grab num {} success", m_id, ret.num());
		if (plan > ret.num()) {
			m_algResult.num = ret.num();
		} else {
			m_algResult.num = plan;
		}
		if (m_algResult.num <= 0) {
			log_error("Next grab num is <= 0");
			return false;
		}
	} else {
		log_error("get next grab num failed");
		return false;
	}
	result.clear();
	param.num = m_algResult.num;
	ret = m_alg->Calculate(param.toByteArray(), m_algResult.pos, result);
	if (ret) {
		log_error("get next grab pos[{}, {}, {},] successed", m_algResult.pos.x,
			m_algResult.pos.y, m_algResult.pos.z);
	} else {
		m_algResult.clear();
		log_error("{} calcuate index {} failed", m_id, index);
		return false;
	}

	m_algResult.index = index;
	return true;
}
/*获取下次最大放货数量*/
int StackPlate::GetNextGrabNum(int index)
{
	if (index == m_algResult.index) {
		return m_algResult.num;
	}
	log_error("{} calculate index is {}, not equal to input index {}", m_id, m_algResult.index, index);
	return -1;
}

/*获取下次最大放货位置*/
bool StackPlate::GetNextGrabPosition(int index, Position& pos)
{
	if (index == m_algResult.index) {
		pos = m_algResult.pos;
		return true;
	}
	pos = {};
	log_error("{} calculate index is {}, not equal to input index {}", m_id, m_algResult.index, index);
	return false;
}

#define PLATE_DI_VAL ('1')

/*托盘到位DI校验*/
bool StackPlate::CheckDI(bool state)
{
	if (m_di <= 0) {
		return true;
	}
	QByteArray output;
	m_rob->ExcuteCmd((int)RobotCmdType::robot_cmd_get_di, QString::number(m_di - 1).toUtf8(), output);
	if (output.size() != 1) {
		log_error("Get DI info failed, ouput = {}", output);
		return false;
	}
	if (state) {
		if (output[0] == PLATE_DI_VAL) {
			return true;
		} else {
			log_error("di {}, state = {}", output[0], state ? "true" : "false");
			return false;
		}
	} else {
		if (output[0] != PLATE_DI_VAL) {
			return true;
		} else {
			log_error("di {}, state = {}", output[0], state ? "true" : "false");
			return false;
		}
	}

}

/*初始化托盘信息*/
bool StackPlate::InitPlateState()
{
	QByteArray data;
	if (plateSql->ReadPlateInfo(m_id, data)) {
		PlateStateJ state = {};
		state.fromJson(data);
		m_state.TransFormPlateStateJ(state);
		if (m_state.num == 0) {	// 如果物料数量为0直接将物料信息清除
			m_state.sku.clear();
		}
		if (m_state.sku.valied) {
			InitAlgSkuInfo();
		}
		return true;
	}
	log_error("read Plate Info from sql failed");
	m_state.clear();
	PlateStateJ info = m_state.TranToPlateStateJ();
	info.id = m_id;
	plateSql->InsertPlate(m_id, info.toByteArray());
	return false;
}

/*保存托盘信息到数据k*/
bool StackPlate::SavePlateInfoToSql()
{
	PlateStateJ state = m_state.TranToPlateStateJ();
	state.id = m_id;
	if (!plateSql->UpdatePlateInfo(m_id, state.toByteArray())) {
		log_error("{} state write to sql failed", m_id);
		return false;
	}

	return true;
}

